#include "Motor.h"

// 电机控制器
MotorControl::MotorControl() {
}

void MotorControl::init() {
  Serial.println("I2C初始化");
  Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);  // 初始化 I2C
  pwm.begin();
  pwm.setPWMFreq(SERVO_FREQ);

  Serial.println("NRF初始化");
  nrf.init(ID, RF24_PA_MIN);
}

// 读取收到的数据并写入tank_kvs
void MotorControl::read() {
  if (nrf.radio.available()) {
    nrf.radio.read(&tank_kvs, sizeof(tank_kvs));
  }
}

// 控制舵机
void MotorControl::loadServo() {
  if (tank_kvs.R_up == LOW) {
    servo_Left_Set();
    servo_Right_Set();
    servo_Up_Set();
    servo_down_Set();
  } else {
    // 叉臂上
    if (tank_kvs.up == LOW) {
      raise_hand();
    } else {
      // servo_Left_Set(servo_left.angle);
    }

    // 叉臂右
    if (tank_kvs.right == LOW) {
      strech_hand();
    } else {
      // servo_Left_Set(servo_left.angle);
    }

    // 叉臂左
    if (tank_kvs.left == LOW) {
      shink_hand();
    } else {
      // servo_Right_Set(servo_right.angle);
    }

    // 叉臂下
    if (tank_kvs.down == LOW) {
      drop_hand();
    } else {
      // servo_Right_Set(servo_right.angle);
    }

    // 夹子 紧
    if (tank_kvs.b == 0) {
      shake_hand();
    } else {
      // servo_Up_Set(servo_up.angle);
    }

    // 夹子 松
    if (tank_kvs.o == 0) {
      let_hand();
    } else {
      // servo_Up_Set(servo_up.angle);
    }
  }
}

// 控制电机
void MotorControl::loadMotor() {
  int min = 15;
  int max = 255;

  // 前进
  if (tank_kvs.RY < -min) {
    clearSw();
    motor_open_type = motor_forward;
    forward(map(tank_kvs.RY, 0, -100, min, max));
  }

  // 后退
  if (tank_kvs.RY > min) {
    clearSw();
    motor_open_type = motor_backward;
    backward(map(tank_kvs.RY, 0, 100, min, max));
  }

  if (tank_kvs.RX < -min) {
    clearSw();
    left_sw = true;
    if (tank_kvs.L_up == LOW) {
      // 左漂移
      motor_open_type = motor_left_drifting;
      left_drifting(map(tank_kvs.RX, 0, -100, min, max));
    } else {
      // 左旋转
      motor_open_type = motor_left_rotate;
      left_rotate(map(tank_kvs.RX, 0, -100, min, max));
    }
  }

  if (tank_kvs.RX > min) {
    clearSw();
    right_sw = true;
    if (tank_kvs.L_up == LOW) {
      // 右漂移
      motor_open_type = motor_right_drifting;
      right_drifting(map(tank_kvs.RX, 0, 100, min, max));
    } else {
      // 右旋转
      motor_open_type = motor_right_rotate;
      right_rotate(map(tank_kvs.RX, 0, 100, min, max));
    }
  }

  // 左平移
  if (tank_kvs.LX < -min) {
    clearSw();
    left_sw = true;
    motor_open_type = motor_left_trans;
    left_trans(map(tank_kvs.LX, 0, -100, min, max));
  }

  // 右平移
  if (tank_kvs.LX > min) {
    clearSw();
    right_sw = true;
    motor_open_type = motor_right_trans;
    right_trans(map(tank_kvs.LX, 0, 100, min, max));
  }

  if ((tank_kvs.RY >= -min && tank_kvs.RY <= min) && (tank_kvs.RX >= -min && tank_kvs.RX <= min) && (tank_kvs.LX >= -min && tank_kvs.LX <= min)) {
    clearSw();
    stop();
  }
}

void MotorControl::loadBuzzer() {
}

void MotorControl::clearSw() {
  left_sw = false;
  right_sw = false;
}

// 前进
void MotorControl::forward(int speed) {
  motor_a(0, speed);
  motor_b(0, speed);
  motor_c(speed, 0);
  motor_d(speed, 0);
}

// 后退
void MotorControl::backward(int speed) {
  motor_a(speed, 0);
  motor_b(speed, 0);
  motor_c(0, speed);
  motor_d(0, speed);
}

// 左旋转
void MotorControl::left_rotate(int speed) {
  motor_a(speed, 0);
  motor_b(speed, 0);
  motor_c(speed, 0);
  motor_d(speed, 0);
}

// 右旋转
void MotorControl::right_rotate(int speed) {
  motor_a(0, speed);
  motor_b(0, speed);
  motor_c(0, speed);
  motor_d(0, speed);
}

// 左漂移
void MotorControl::left_drifting(int speed) {
  motor_a(speed, 0);
  motor_b(0, 0);
  motor_c(speed, 0);
  motor_d(speed, 0);
}

// 右漂移
void MotorControl::right_drifting(int speed) {
  motor_a(0, speed);
  motor_b(0, speed);
  motor_c(0, speed);
  motor_d(0, 0);
}

// 左平移
void MotorControl::left_trans(int speed) {
  motor_a(0, speed);
  motor_b(speed, 0);
  motor_c(0, speed);
  motor_d(speed, 0);
}

// 右平移
void MotorControl::right_trans(int speed) {
  motor_a(speed, 0);
  motor_b(0, speed);
  motor_c(speed, 0);
  motor_d(0, speed);
}

// 停止
void MotorControl::stop() {
  motor_a(0, 0);
  motor_b(0, 0);
  motor_c(0, 0);
  motor_d(0, 0);
}

void MotorControl::buzzer_set(int value) {
  // digitalWrite(BUZZER_PIN, value);
  // analogWrite(BUZZER_PIN, BUZZER_MAX_HZ);
}

// 控制电机a
void MotorControl::motor_a(int in1, int in2) {
  analogWrite(MOTOR_A1_PIN, in1);
  analogWrite(MOTOR_A2_PIN, in2);
}

// 控制电机b
void MotorControl::motor_b(int in1, int in2) {
  analogWrite(MOTOR_B1_PIN, in1);
  analogWrite(MOTOR_B2_PIN, in2);
}

// 控制电机c
void MotorControl::motor_c(int in1, int in2) {
  analogWrite(MOTOR_C1_PIN, in1);
  analogWrite(MOTOR_C2_PIN, in2);
}

// 控制电机d
void MotorControl::motor_d(int in1, int in2) {
  analogWrite(MOTOR_D1_PIN, in1);
  analogWrite(MOTOR_D2_PIN, in2);
}

// 叉臂举起
void MotorControl::raise_hand() {
  // Serial.println("叉臂举起");
  int val = servo_left.angle + _step;
  if (val > servo_left.max_angle) {
    servo_left.angle = servo_left.max_angle;
  } else {
    servo_left.angle = val;
  }
  servo_Left_Set(servo_left.angle);
}

// 叉臂向下
void MotorControl::drop_hand() {
  // Serial.println("叉臂向下");
  int val = servo_left.angle - _step;
  if (val < servo_left.min_angle) {
    servo_left.angle = servo_left.min_angle;
  } else {
    servo_left.angle = val;
  }
  servo_Left_Set(servo_left.angle);
}

// 叉臂伸展
void MotorControl::strech_hand() {
  // Serial.println("叉臂伸展");
  int val = servo_right.angle + _step;
  if (val > servo_right.max_angle) {
    servo_right.angle = servo_right.max_angle;
  } else {
    servo_right.angle = val;
  }
  servo_Right_Set(servo_right.angle);
}

// 叉臂收回
void MotorControl::shink_hand() {
  // Serial.println("叉臂收回");
  int val = servo_right.angle - _step;
  if (val < servo_right.min_angle) {
    servo_right.angle = servo_right.min_angle;
  } else {
    servo_right.angle = val;
  }
  servo_Right_Set(servo_right.angle);
}

// 夹子抓取
void MotorControl::shake_hand() {
  // Serial.println("夹子抓取");
  int val = servo_up.angle - _step;
  if (val < servo_up.min_angle) {
    servo_up.angle = servo_up.min_angle;
  } else {
    servo_up.angle = val;
  }
  servo_Up_Set(servo_up.angle);
}

// 夹子松开
void MotorControl::let_hand() {
  // Serial.println("夹子松开");
  int val = servo_up.angle + _step;
  if (val > servo_up.max_angle) {
    servo_up.angle = servo_up.max_angle;
  } else {
    servo_up.angle = val;
  }
  servo_Up_Set(servo_up.angle);
}

void MotorControl::servo_Left_Set() {
  servo_Left_Set(servo_left.initial_angle);
}
void MotorControl::servo_Left_Set(int angle) {
  servo_angle(SERVO_LEFT_PIN, servo_left, angle);
}

void MotorControl::servo_Right_Set() {
  servo_Right_Set(servo_right.initial_angle);
}
void MotorControl::servo_Right_Set(int angle) {
  servo_angle(SERVO_RIGHT_PIN, servo_right, angle);
}

void MotorControl::servo_Up_Set() {
  servo_Up_Set(servo_up.initial_angle);
}
void MotorControl::servo_Up_Set(int angle) {
  servo_angle(SERVO_UP_PIN, servo_up, angle);
}

void MotorControl::servo_down_Set() {
  servo_down_Set(servo_down.initial_angle);
}
void MotorControl::servo_down_Set(int angle) {
  servo_angle(SERVO_DOWN_PIN, servo_down, angle);
}

void MotorControl::servo_angle(int pin, SERVO_PARAM obj, int angle) {
  int value = map(obj.angle, 0, 180, SERVOMIN, SERVOMAX);
  // Serial.print("PIN ");
  // Serial.print(pin);
  // Serial.print("\t min_angle ");
  // Serial.print(obj.min_angle);
  // Serial.print("\tangle ");
  // Serial.print(obj.angle);
  // Serial.print("\tpwm ");
  // Serial.println(value);
  pwm.setPWM(pin, 0, value);
}

// 单击
bool MotorControl::pressed(int key, bool default_state)
{
  // 单次按键消抖
  if (key != default_state && !_isPressed_flag)
  { // 按钮刚被按下, 且按下标记为False, 防止瞬间多次执行
    delay(20);
    if (key != default_state)
    { // 确认按钮仍然被按下
      _isPressed_flag = true;
      return true;
    }
  }

  else if (key == default_state)
  {                          // 按钮释放时重置按钮状态
    _isPressed_flag = false; // 更新按钮状态为未按下
  }

  return false;
}